#include <iostream>
#include <unistd.h>
#include <math.h>
#include <ctime>

#include "leg_ctrl.h"
#include "common.h"

using namespace std;

// Leg_Ctrl::Leg_Ctrl(uint8_t motor_n, uint8_t id_array[], Motor_Type type_array[])
Leg_Ctrl::Leg_Ctrl(Leg_Motor_Desc &lmotor_desc, Leg_Can_Desc &lcan_desc, Leg_IK_Desc &lik_desc, Leg_Trace_Desc &ltrace_desc)
{
    cout<<"Leg_Ctrl"<<endl;
    this->p_can_ctrl    = new Can_Ctrl();
    this->p_motor_ctrl  = new Motor_Ctrl(p_can_ctrl, lmotor_desc.motor_n, lmotor_desc.id_array, lmotor_desc.type_array);
    this->p_ik_obj      = new IK_Obj(p_motor_ctrl, L_ABCD, L_HIP, L_KNEE);
    this->p_trace_obj   = new Trace_Cycloid(ltrace_desc.type, ltrace_desc.param, ltrace_desc.len);
}

Leg_Ctrl::~Leg_Ctrl()
{
    cout<<"~Leg_Ctrl"<<endl;
    delete p_trace_obj;
    delete p_ik_obj;
    delete p_motor_ctrl;
    delete p_can_ctrl; 
}

/* 电机旋转至指定位置 target_start为起点，target_rotate为目的旋转角度，target_value[x][1]为旋转终点角度 */
int32_t Leg_Ctrl::leg_rotate(double target_start[], double target_rotate[], double target_value[][3])
{
#define END_NO_WAIT
    int32_t ret = 0;
    uint8_t time = 0;
    const uint8_t motor_num = this->p_motor_ctrl->get_motor_num();
    double cur_pos[motor_num];
    double target_end[motor_num];
    double freq[motor_num];
    double each_max[motor_num];
    double accept_err[motor_num];
    uint32_t duration[motor_num];
    uint32_t duration_max = 0;
    double freq_max = 0.0f;

    Dir_Type rotate_dir[motor_num];
    double rotate_offset[motor_num];
    for(uint8_t i=0; i<motor_num; i++) {
        rotate_dir[i]      =  this->p_ik_obj->get_ik_param()->theta_dir[i];
        rotate_offset[i]   =  this->p_ik_obj->get_ik_param()->theta_offset[i];
    }

    /* 发送频率统一,取较慢者 */
    for(uint8_t i=0; i<motor_num; i++) {
        freq[i]         = this->p_motor_ctrl->get_motor_item(i)->get_motor_cmd_freq();
        if(freq_max < freq[i]) {
            freq_max = freq[i];
        }
    }
    for(uint8_t i=0; i<motor_num; i++) {
        target_end[i]   = target_value[i][0];
        each_max[i]     = this->p_motor_ctrl->get_motor_item(i)->get_motor_each_max();
        accept_err[i]   = this->p_motor_ctrl->get_motor_item(i)->get_motor_acceptable_err();
        duration[i]     = (((target_rotate[i] > 0.0f ? target_rotate[i] : -target_rotate[i])/target_value[i][1]) / freq_max);
    }

    /* 临时处理：电机0负载太重了，因此阈值留够，另外在摆线运动过程中电机0本来就不需要转 */
    accept_err[0] = 1.5f;
    
    /* 调整，防止单次旋转角度过大 ,另外，旋转角度过小则跳过*/
    for(uint8_t i=0; i<motor_num; i++) {
        if((target_rotate[i] > ((-1)*accept_err[i])) && (target_rotate[i] < accept_err[i])) {
            duration[i] = 0;
            freq[i] = 0;
        } else if(((target_rotate[i] > 0.0f ? target_rotate[i] : -target_rotate[i])/duration[i]) > each_max[i]) {                    //每次度数
            duration[i] = ((target_rotate[i] > 0.0f ? target_rotate[i] : -target_rotate[i])/each_max[i]);
            freq[i] = each_max[i]/target_value[i][1];
        } 
    }

    /* duration_max和freq取最大*/
    for(uint8_t i=0; i<motor_num; i++) {
        if(duration_max < duration[i]) {
            duration_max = duration[i];
        }
        if(freq_max < freq[i]) {
            freq_max = freq[i];
        }
    }

    double percent = (double)1.0f/duration_max;

    std::time_t t0 = std::time(0);

    /* 插值法旋转 */
    for(uint32_t j = 1; j <= duration_max; j++) {
        for(uint8_t i=0; i<motor_num; i++) {
            if(duration[i] == 0) {
                continue;
            }
            target_value[i][0] = ((1-j*percent)*target_start[i]) + (j*percent * target_end[i]);
            this->p_motor_ctrl->motor_func(i, MOTOR_MODE_MIX, target_value[i], 3);
        }
        usleep(freq_max*1000*1000);
    }
    // sleep(1);
    for(uint8_t i=0; i<motor_num; i++) {
        target_value[i][0] = target_end[i];
    }

    /* 最后检查位置，若仍未达到，继续旋转*/
    for(uint8_t i=0; i<motor_num; i++) {
        time = 0;
        while(1) {
            cur_pos[i] = this->p_motor_ctrl->get_motor_item(i)->get_cur_pos();
            if((cur_pos[i] > (target_end[i] + each_max[i])) || (cur_pos[i] < (target_end[i] - each_max[i]))) { //旋转失败，做保护，防止电机一下转的角度太大 
                ret = -1;
                break;
            }
            if((cur_pos[i] <  (target_end[i] + accept_err[i]))  && (cur_pos[i] >  (target_end[i] - accept_err[i]))) {   //说明旋转ok
                break;
            } else {
                this->p_motor_ctrl->motor_func(i, MOTOR_MODE_MIX, target_value[i], 3);
                usleep(each_max[i]/target_value[i][1]*1000*1000);
            #ifndef END_NO_WAIT
                time++;
                if(time > 3) {      //如果3次还没有旋转到，就退出
                    time = 0;
                    ret = -1;
                    break;
                }
            #else
                break;
            #endif
            }
        }
    }

    std::time_t t1 = std::time(0);
    // sleep(1);
    for(uint8_t i=0; i<motor_num; i++) {
        cur_pos[i] = this->p_motor_ctrl->get_motor_item(i)->get_cur_pos();
        printf("[motor%d] start:%f, target:%f, cur:%f;\t\
                [ik] start:%f, target:%f, cur:%f;\t\
                rotate:%f, time_use:%lds\n", \
                i, target_start[i], target_end[i], cur_pos[i], \
                target_start[i]*rotate_dir[i]+rotate_offset[i], target_end[i]*rotate_dir[i]+rotate_offset[i], cur_pos[i]*rotate_dir[i]+rotate_offset[i], \
                target_rotate[i], t1 - t0);
    }
    return ret;
}

/* 小角度旋转，不关注是否转到位置 */
int32_t Leg_Ctrl::leg_rotate_little(double target_start[], double target_rotate[], double target_value[][3])
{
    const uint8_t motor_num = this->p_motor_ctrl->get_motor_num();
    double each_max[motor_num];
    double freq[motor_num];
    double freq_max = 0.0f;

    for(uint8_t i=0; i<motor_num; i++) {
        freq[i]         = this->p_motor_ctrl->get_motor_item(i)->get_motor_cmd_freq();
        each_max[i]     = this->p_motor_ctrl->get_motor_item(i)->get_motor_each_max();
        if(freq_max < freq[i]) {
            freq_max = freq[i];
        }
    }
    for(uint8_t i=0; i<motor_num; i++) {
        if(target_rotate[i] > each_max[i]) {
            return -1;
        }
        this->p_motor_ctrl->motor_func(i, MOTOR_MODE_MIX, target_value[i], 3);
    }
    // usleep(freq_max*1000*1000);

    return 0;
}

/* 根据运动轨迹，用逆运算生成姿态角度 */
void *leg_move_thread(void *param)
{
    Leg_Ctrl *leg_ctrl = (Leg_Ctrl *)param;
    const uint8_t motor_num = leg_ctrl->p_motor_ctrl->get_motor_num();

    /* 0. 等待线程使能 */
    uint8_t count = 0;
    while(0 == leg_ctrl->leg_thread_getflag()) {
        sleep(1);
        ++count;
        if(count >= 10) {
            printf("leg_move thread is disable\n");
            return NULL;
        }
    }

    /* 1. 零点位置提前定义好，确定电机角度和运动学角度之间的换算系数*/
    /* 运动学角度和电机角度之间的参数 */
    Dir_Type rotate_dir[motor_num];
    double rotate_offset[motor_num];
    for(uint8_t i=0; i<motor_num; i++) {
        rotate_dir[i]      =  leg_ctrl->p_ik_obj->get_ik_param()->theta_dir[i];
        rotate_offset[i]   =  leg_ctrl->p_ik_obj->get_ik_param()->theta_offset[i];
    }

    /* 2. 摆好腿的位置，即初始摆放位置,需要注意安全。另外，从结构出发，小腿的角度需要放到运动学[-90，-180]位置范围内*/
        //人为操作一下...

    /* 3. 获取当前各个关节角度，并旋转到轨迹运动的初始化位置 */
    /* 摆线运动初始位置(运动学). */
    double r = sqrt(pow(leg_ctrl->p_ik_obj->get_lhip(), 2) + pow(leg_ctrl->p_ik_obj->get_lknee(), 2))/(2*PAI);
    double trace_param[TRACE_PARAM_MAX] = {           //{r,w,x y,z} = {12.5f, 60.0f, -12.5f, 6.35f, -21.65f}
        r, \
        360.0f,  \
        (-1)*PAI*r,    \
        leg_ctrl->p_ik_obj->get_labcd(),      \
        (-1)*sqrt(3)*PAI*r,
    };
    leg_ctrl->p_trace_obj->trace_param_sync(trace_param);

    /* pos[] 和 angle[]均为运动学的值 */
    double *angle = leg_ctrl->p_ik_obj->get_ik_param()->angle;
    double *pos = leg_ctrl->p_ik_obj->get_ik_param()->pos;
    pos[0] = trace_param[2];
    pos[1] = trace_param[3];
    pos[2] = trace_param[4];
    // angle[0] = 0.0f;
    // angle[1] = 67.0f;
    // angle[2] = -90.0f;
    leg_ctrl->p_ik_obj->inverse_kinematics();
    cout << "init pos:" << pos[0] << " " << pos[1] << " " << pos[2] << endl;
    cout << "init angl:" << angle[0] << " " << angle[1] << " " << angle[2] << endl;

    /* 电机旋转参数的值 , 需要将运动学角度向电机角度转化 */
    double start_pos[motor_num];            //起始角度
    double target_rotate[motor_num];        //目的旋转角度
    double target_value[motor_num][3];      //电机参数
    
    for(uint8_t i=0; i<motor_num; i++) {
        target_value[i][0] =  rotate_dir[i]*(angle[i] - rotate_offset[i]);     //旋转终点角度，需将运动学角度向电机角度转化
        target_value[i][1] =  90.0f;        //角速度
        target_value[i][2] =  90.0f;        //角加速度
    }

    /* 电机初始化 进入伺服模式 */  
    for(uint8_t i=0; i<motor_num; i++) {
        leg_ctrl->p_motor_ctrl->motor_func(i, MOTOR_MODE_EXIT, NULL, 0);
        usleep(100);
    }
    sleep(3);
    for(uint8_t i=0; i<motor_num; i++) {
        leg_ctrl->p_motor_ctrl->motor_func(i, MOTOR_MODE_ENTER, NULL, 0);
        usleep(100);
    }
    sleep(3);

    /* 运动至摆线运动初始位置 */
    for(uint8_t i=0; i<motor_num; i++) {
        start_pos[i] = leg_ctrl->p_motor_ctrl->get_motor_item(i)->get_cur_pos();
        target_rotate[i] = target_value[i][0] - start_pos[i];
    }
    leg_ctrl->leg_rotate(start_pos, target_rotate, target_value);
    sleep(1);
    
    /* 4. 根据轨迹，用逆运动生成角度，开始进行完整的摆线运动 */

#if 1
    #define TRACE_TIME  5
    uint32_t cnt = 0;
    double *trace_pos;
    double timeval = 0.002f;
    double time = 0;

    std::time_t t0 = std::time(0);
    while(1 == leg_ctrl->leg_thread_getflag()) {
        cnt = leg_ctrl->p_trace_obj->trace_generate(time);
        trace_pos = leg_ctrl->p_trace_obj->get_pos();
        pos[0] = trace_pos[0]; 
        pos[1] = trace_pos[1];
        pos[2] = trace_pos[2];
        leg_ctrl->p_ik_obj->inverse_kinematics();
        // printf("pos:%f, %f, %f\t", trace_pos[0], trace_pos[1], trace_pos[2]);
        // printf("ang:%f, %f, %f\n", angle[0], angle[1], angle[2]);

        /* 检查角度是否非法 */
        if(0 == leg_ctrl->p_ik_obj->is_angle_valid()) {
            printf("motor angle is invaild, %f,%f,%f\n", angle[0], angle[1], angle[2]);
            break;
        }

        /* 转起来 */
        for(uint8_t i=0; i<motor_num; i++) {
            start_pos[i]        = leg_ctrl->p_motor_ctrl->get_motor_item(i)->get_cur_pos();
            target_value[i][0]  = rotate_dir[i]*(angle[i] - rotate_offset[i]); ;     //旋转终点角度
            target_rotate[i]    = target_value[i][0] - start_pos[i];    
        }
        leg_ctrl->leg_rotate_little(start_pos, target_rotate, target_value);
        time += timeval;
        
        if(cnt == TRACE_TIME) {  //轨迹运动完毕
            printf("leg trace ok\n");
            leg_ctrl->leg_thread_disable();
        }

        usleep(timeval*1000*1000);
    }
    std::time_t t1 = std::time(0);
    printf("trace%d time_use:%lds\n", cnt, t1 - t0);

#else

    while(1 == leg_ctrl->leg_thread_getflag()) {
        sleep(1);
    }    

#endif
    printf("leg_move thread exit\n");
    return NULL;
}


int Leg_Ctrl::leg_ctrl_init()
{
    if((p_can_ctrl == nullptr) || (p_motor_ctrl == nullptr)) {
        cout << "NOTE: p_can_ctrl or p_motor_ctrl is null" << endl;
        return -1;
    }

    if(0 != p_trace_obj->trace_init()) {
        cout << "NOTE: trace_init fail!" << endl;
        return -1;
    }

    if(0 != p_ik_obj->ik_init()) {
        cout << "NOTE: ik_init fail!" << endl;
        return -1;
    } 

    if(0 != p_can_ctrl->can_ctrl_init()) {
        cout << "NOTE: can_init fail!" << endl;
        return -1;
    }

    if(0 != p_motor_ctrl->motor_ctrl_init()) {
        cout << "NOTE: motor_ctrl_init fail!" << endl;
        return -1;
    } 
    
    pthread_create(&pid, NULL, leg_move_thread, this);
    ready = 1;

    return 0;
}

int Leg_Ctrl::leg_ctrl_deinit()
{
    if(ready) {
        pthread_join(pid, NULL);
        p_trace_obj->trace_deinit();
        p_ik_obj->ik_deinit();
        p_motor_ctrl->motor_ctrl_deinit();
        p_can_ctrl->can_ctrl_deinit();
    }

    ready = 0;

    return 0;
}